require("sys")
local dcmd = {}
local shift = 77
local speedZero=69
-- 是否是返回
local  backSeedState=false 
local function CmdMain()
    local function onSpeed(speed)
        if speed == nil then
            return
        end
        -- （71-91） 前进  停止是69  后退是67到50
        local factSpeed = speedZero
        if speed > 0 then
            backSeedState=false
            factSpeed=math.floor(speed/5)+71
        end
        if speed < 0 then
            factSpeed=67+math.floor(speed/5.8)
            if not backSeedState then
                backSeedState=true
                pwm.open(6, 50, factSpeed, 0, 1000)
                -- pwm.open(1, 50, factSpeed, 0, 1000)
                sys.wait(10)
                pwm.open(6, 00, speedZero, 0, 1000)
                -- pwm.open(1, 50, speedZero, 0, 1000)
                sys.wait(30)
                pwm.open(6, 50, factSpeed, 0, 1000)
                -- pwm.open(1, 50, factSpeed, 0, 1000)
                sys.wait(30)
                pwm.open(6, 50, speedZero, 0, 1000)
                -- pwm.open(1, 50, speedZero, 0, 1000)
                sys.wait(30)
            end
        end
        pwm.open(6, 50, factSpeed, 0, 1000)
        -- pwm.open(1, 50, factSpeed, 0, 1000)
        log.info("set speed to : ", speed)
    end
    local function onAngle(angle)
        if angle == nil then
            return
        end
        angle = angle + shift
        log.info("set angle to : ", angle)
        pwm.open(13, 50, angle, 0, 1000)
    end
    local function onCmd(dat)
        local function doCtrl(speed, angle)
            onSpeed(dat.dat.speed)
            onAngle(dat.dat.angle)
        end

        local function onBle(dat)
            if (dat.src == "dble") then
                log.info("------蓝牙命令来了------", dat.dat.speed, dat.dat.angle)
                doCtrl(dat.dat.speed, dat.dat.angle)
            end
        end
        if (dat.evt == "COMMAND") then
            onBle(dat)
        end
    end

    -- pwm.open(1, 500, 0, 0, 100)
    pwm.open(6, 50, 0, 0, 1000)
    pwm.open(13, 50, shift, 0, 1000)
    while true do
        local ret, dat = sys.waitUntil("ddn_event")
        if (ret == true) then
            onCmd(dat)
        end
    end
end

local function Init()
    log.info("-------dcmd init----------")
    sys.taskInit(CmdMain)
end
dcmd.Init = Init
return dcmd
